function [ flowX flowY ] = HornSchunkRobust(imageSeq, params)
    params.numScales = 1 + floor(log(min(size(imageSeq{1}, 1), size(imageSeq{1}, 2)) / 16) / log(params.pyramidScaleFactor));

    for iIm = 1 : numel(imageSeq) - 1        
        im1F = im2double(MyRGB2Gray(imageSeq{iIm}));
        im2F = im2double(MyRGB2Gray(imageSeq{iIm + 1}));         
                 
         pyramid1 = ImagePyramid(im1F, params);
         pyramid2 = ImagePyramid(im2F, params);
         
         u = zeros(size(pyramid1{params.numScales}));
         v = u;
         for iScale = params.numScales : -1 : 1             
             im1 = pyramid1{iScale};
             im2 = pyramid2{iScale};
             [ x y ] = meshgrid(1 : size(im1, 2), 1 : size(im2, 1));            
             x2 = x + u;
             y2 = y + v;
             
             im2Warped = interp2(x, y, im2, x2, y2);        
             
             du = zeros(size(pyramid1{iScale}));
             dv = du;
             
             for iIter = 1 : params.numNonLinIterations
                [ lhsData lhsSmooth rhs ] = Equations(im1, im2Warped, u, v, du, dv, params);
                lhs = lhsData - params.alpha * lhsSmooth;
                dFlow = lhs \ rhs;
                du = reshape(dFlow(1 : numel(dFlow) / 2), size(im2));
                dv = reshape(dFlow(1 + numel(dFlow) / 2 : end), size(im2));  
                
%                 if max(sqrt((du(:) - du1(:)).^2 + (dv(:) - dv1(:)).^2)) < 1e-2
%                     break;
%                 else
%                     du = du1;
%                     dv = dv1;                    
%                 end
             end

             u = u + du;
             v = v + dv;            

             if (params.medianFilterSize > 0)
                 u = medfilt2(u, params.medianFilterSize);
                 v = medfilt2(v, params.medianFilterSize);                       
             end             
             
             if (iScale > 1)
                 u = imresize(u, size(pyramid1{iScale - 1}));
                 v = imresize(v, size(pyramid1{iScale - 1}));           
             end
             
             if params.isVerbose
                fprintf(1, 'Horn Schunk: Image %d/%d | Scale %d/%d Done.\n', iIm, ...
                    numel(imageSeq) - 1, params.numScales - iScale + 1, params.numScales);
             end
         end         
        
        if (numel(imageSeq) > 2)        
            flowX{iIm} = u;
            flowY{iIm} = v;
        else
            flowX = u;
            flowY = v;
        end
        
        if params.isVerbose
            fprintf(1, 'Horn Schunk: Image %d/%d Done.\n', iIm, numel(imageSeq) - 1);
        end
    end
end

function pyramid = ImagePyramid(image, params)
    pyramid = { image };
    h = fspecial('gaussian', params.preSmoothingWindow, params.preSmoothingSigma);    
    for i = 2 : params.numScales
        curImage = imfilter(pyramid{i - 1}, h, 'symmetric');
        curImage = imresize(curImage, 1 / params.pyramidScaleFactor);
        pyramid{i} = curImage;
    end
end

function y = RobustPenalizer(x, request)
    myEps = 1e-4;
    if request == 0 % Value
        y = sqrt(x + myEps.^2);
    elseif request == 1 % 1st derivative        
        y = 1 ./ sqrt(x + myEps.^2);
    elseif request == 2 % 2nd derivative
        throw;
    else
        throw;
    end
end

function [  lhsData lhsSmooth rhs ] = Equations(I1, I2, u, v, du, dv, params)
    [ Ix Iy It  Ixx Iyy Ixy Ixt Iyt ] = HSGradients(I1, I2, params);    
    
    It1 = (It + Ix .* du + Iy .* dv) .^2;
    dataPenalty = RobustPenalizer(It1, 1);    
    lhsData = DataTermMatrix(Ix, Iy, dataPenalty);
    
    smoothPenalty = RobustPenalizer((u + du).^2 + (v + dv).^2, 1);
    lhsSmooth = SmoothnessTermMatrix(size(I2), smoothPenalty);   

    rhs = params.alpha * lhsSmooth * [ u(:) ; v(:) ] ...
       - [ dataPenalty(:) .* Ix(:).*It(:); dataPenalty(:) .* Iy(:).*It(:) ];
end

function dataTerm = DataTermMatrix(Ix, Iy, penalty)    
    n = numel(Ix);
    constTerm = ones(numel(Ix), 1) * 1e-4;
    sIx2   = spdiags(constTerm + penalty(:) .* Ix(:).^2, 0, n, n);
    sIy2   = spdiags(constTerm + penalty(:) .* Iy(:).^2, 0, n, n);
    sIxy   = spdiags(penalty(:) .* Ix(:).*Iy(:), 0, n, n);
    dataTerm = [ sIx2, sIxy; sIxy, sIy2 ];   
end

function sMat = SmoothnessTermMatrix(imSize, smoothPenalty)
    m = imSize(1);
    n = imSize(2);      
    
        
    mask = [ 0 1 0 ; 1 0 1 ; 0 1 0 ];
    [ maskRow maskCol ] = find(mask);
    
    numEntries = (4*(n-2)*(m-2)+3*2*(n-2+m-2)+4*2+n*m);
    rowInd = zeros(1, numEntries);
    colInd = zeros(1, numEntries);
    data = zeros(1, numEntries);
    
    dataCounter = 1;
    for col = 1 : n
        for row = 1 : m
            curMaskRow = maskRow + row - 2;
            curMaskCol = maskCol + col - 2;

            goodLocs = find((curMaskRow > 0) .* (curMaskCol > 0) .* (curMaskRow <=m) .* (curMaskCol <= n));
                        
            curPenalty = zeros(4, 1);    
            goodPenaltyInd = (curMaskCol(goodLocs) - 1) * m + curMaskRow(goodLocs);
            curPenalty(goodLocs) = smoothPenalty(goodPenaltyInd);
            curPenalty(goodLocs) = 0.5 * (curPenalty(goodLocs) + smoothPenalty(row, col));
            
            neigInd = (curMaskCol(goodLocs) - 1) * m + curMaskRow(goodLocs);            
            curInd = (col - 1) * m + row;            
            curRowInd = curInd(ones(1, 1 + numel(goodLocs))); 
            
            curAssignInd = dataCounter : dataCounter + numel(curRowInd) - 1;
            
            rowInd(curAssignInd) = curRowInd;
            colInd(curAssignInd) = [ curInd, neigInd' ];
            data(curAssignInd) = [-sum(curPenalty(goodLocs))', curPenalty(goodLocs)' ];
            dataCounter = max(curAssignInd) + 1;
        end
    end
    
    smoothingMat = sparse(rowInd, colInd, data, m*n, m*n);
    sMat = [ smoothingMat, sparse(m*n, m*n); sparse(m*n, m*n), smoothingMat ];    
end